
#include "global.h"


using namespace cv;
using namespace std;

Mat target_stack;
vector<Point2f> target_stack_vect;

double angular_vel = 0;
double linear_vel = 0;

char flg;

ArRobot robot;

vector<rb_sonar> rb_sonars;

decision deci;


float getTargetPosition()
{
	/*
	char *centralhost="140.118.38.92";
	double pose[3];


	myIPC_Init(centralhost);
	Sleep(200);

	// get robot heading
	myGetRobotPose(pose);
	Sleep(200);
	printf("%f  %f  %f \n", pose[0], pose[1], pose[2]);

	// get delta angle
	// where's the function


	// do the minus

	myShutdown();	
	system("pause");
	*/

	return 1;
}


float getObstaclePosition() // split this module if getting bigger
{	
	//double ret_val = 1;

	// Get all sonars in range
	rb_sonar sn, ret_sn;

	for (int i=0; i<SONAR_NUM; i++)
	{	 
		//val = robot.getSonarRange(i);
		sn.value = robot.getSonarRange(i);

	  	if (sn.value >= OBS_DIST_THRES) sn.value = INF;

		sn.idx = i;

		rb_sonars.push_back(sn);		
	}	

	// get min Left
	rb_sonar sn_minL = rb_sonars[0];
	if (rb_sonars[1].value < sn_minL.value ) sn_minL = rb_sonars[1];
	if (rb_sonars[2].value < sn_minL.value ) sn_minL = rb_sonars[2];

	// get min Right
	rb_sonar sn_minR = rb_sonars[3];
	if (rb_sonars[4].value < sn_minR.value ) sn_minR = rb_sonars[4];
	if (rb_sonars[5].value < sn_minR.value ) sn_minR = rb_sonars[5];

	// minL vs minR
	// exception A: block one side (L/R)
	const double tolA = 10;
	//if ((sn_minL.idx == 2) && (sn_minR.idx == 3) && (sn_minL.value + tolA >= sn_minR.value) && ())
	if ((sn_minL.idx == 2) && (sn_minR.idx == 3) && (abs(sn_minL.value - sn_minR.value) <= tolA))
	{
		// choose the open side to go
		double sn_L_total = rb_sonars[0].value + rb_sonars[1].value + rb_sonars[2].value;
		double sn_R_total = rb_sonars[3].value + rb_sonars[4].value + rb_sonars[5].value;
		
		if (sn_L_total >= sn_R_total)
		{
			ret_sn = sn_minL;
		} 
		else
		{
			ret_sn = sn_minR;
		}
	}
	// exception B: block U shape
	// later!!
	
	if (sn_minL.value < sn_minR.value) 
		//ret_val = sn_minL.value;
		ret_sn.value = sn_minL.value;
	else 
		ret_sn.value = sn_minR.value;

	// return value;
	return ret_sn.value;	
}

void steering(double steering_param, double velocity_param)
{
	double K = steering_param;

	/*
	if (steering_mode == DECEL_LROT || steering_mode == DECEL_RROT || steering_mode == DECEL_STRT)
	{
		linear_vel = linear_vel - K*DEL_LINEAR_VEL;

		if (steering_mode != DECEL_STRT)
		{
			angular_vel = angular_vel - K*DEL_ANGULAR_VEL;
		}
	}
	else
		if (steering_mode == ACCEL_LROT || steering_mode == ACCEL_RROT || steering_mode == ACCEL_STRT)
		{
			linear_vel = linear_vel + K*DEL_LINEAR_VEL;

			if (steering_mode != DECEL_STRT)
			{
				angular_vel = angular_vel + K*DEL_ANGULAR_VEL;
			}
		}
	*/

	linear_vel = linear_vel + velocity_param;
	angular_vel = angular_vel + steering_param;


	double V = linear_vel;
	double dtheta = angular_vel;

	double wr = (2*V+dtheta*L)/2/r;
	double wl = (2*V-dtheta*L)/2/r;

	robot.setVel2(wl*180/PI,wr*180/PI);

/*
	switch(steering_mode)  
	{
	case (DECEL_LROT):
		cout << "Run Away!";
		break;

	case (DECEL_RROT):
		cout << "Fight Defensively";
		break;

	case (DECEL_STRT):

		break;

	case (ACCEL_LROT):
		cout << "Run Away!";
		break;

	case (ACCEL_RROT):
		cout << "Fight Defensively";
		break;

	case (ACCEL_STRT):

		cout << "All Out Attack!";
		break;

	} // end switch
*/
}


void controller()
{

	// go straight to target
	

	// store a stack of targets


	// create new target if current target is blocked

}


void wrappedOnMouse(int event, int x, int y, int flags, void* ptr)
{
	TargetFeeder* mcPtr = (TargetFeeder*)ptr;
	if(mcPtr != NULL)
		mcPtr->on_mouse(event, x, y, flags, 0);
}


int main(int argc, char* argv[])
{
	
	// Initialization
    float	target_pos, obstacles_pos, potential; // values for input variables
    char	option;	// var for selection of what user wants to do
	int ret_val;

	double steering_param, velocity_param;
	
	char c;

	Mat image;
	char* file = "expmap.jpg";
	image = imread(file, 1);

	//target_stack = Mat::zeros() -> use a  dynamic vector


	// get obstacle positions
	int* out;
	int m, n;
//	myGetObstacleCoord(&out, m, n);

//	Mat obstacles = Mat::zeros(1, m*n/2, CV_32FC2);
	// CREATE RANDOM OBSTACLES
	RNG rng; 
	m = 2; n = 50;
	out=(int*)malloc(m*n*sizeof(int));
	for (int i=0; i<m*n; i++)
	{
		out[i] = rng.uniform(0, image.rows);
		//out[i] = i;
		//out++;
	}

	Mat obstacles = Mat::zeros(1, m*n/2, CV_32FC2);

	for (int i=0; i<m*n; i++)
	{
		if(i%2==0)
		{
			printf("%5d",out[i]);
			obstacles.at<Vec2f>(0,i/2)[0] = out[i];
		}
		else
		{
			printf(" %5d ",out[i]);
			obstacles.at<Vec2f>(0,i/2)[1] = out[i];
		}
	}

	TargetFeeder tf = TargetFeeder(image, obstacles);

	char* wind_name = "Choose Workspace";
//	char c;

	// draw obstacles on image
	drawCircles(tf.map_image, tf.map_obstacles);



	imshow(wind_name, tf.map_image);

	setMouseCallback(wind_name, wrappedOnMouse, (void*)&tf);

	//setMouseCallback(wind_name, on_mouse, 0);   // bug is here! cannot use mouseCallback

	// click 2 corners of workspace

	while (tf.clicked_points.size() < 2)
	{
		printf("Please click 2 POINTS to create workspace!!\n");

		c = cvWaitKey(1);
		if (c == 27) 
		{
			break;
		}
	}

	//	};

	// call generate
	tf.generate(10);

	// visualize trajectory
	tf.visualize();
	
	
	FuzzyController FZCtrl(ret_val); 

	if (ret_val < 0)
	{
		cout << "Error Opening fuzzycontroller.fcl";
		// make sure the "working directory" in "Project | Settings..."
		// is set to the executable's directory if running from the MSVC IDE
	}


	while (1)
	{ 
		// delay delta_t 

		// get obstacle status
		obstacles_pos = getObstaclePosition();

		// get target status
		target_pos = getTargetPosition();

		// calculate potential
		potential = 0;

		// get mode: virtual target OR real target
		
		
		// feed to fuzzy controller to have output decision 
		deci = FZCtrl.getDecision(target_pos, obstacles_pos, potential);

		// execute decision
		// define K value
		steering_param = deci.direction/180*PI;
		velocity_param = deci.velocity;

		
		// call steering
		steering(steering_param, velocity_param);


		// press "Esc" to escape
		c = (char)waitKey(1);
		if (c == 27)
		{
			break;
		}
	}  
	

	waitKey(0);

    return 0;

} // end main()


